/*
 *  Copyright 2011 Elmer Garduno
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 */

import lll.wrj4P5.*;
import lll.Loc.*;

Wrj4P5 wii;
  
float px = 0;
float py = 0;

float CONTROLLERS_Y;

color RED;
color GREEN;
  
boolean ledsSet = false;  
  
void setup()
{
  size(300,320);
  smooth();  
  RED = color(255, 0, 0);
  GREEN = color(0, 255, 0);
  CONTROLLERS_Y = height - 120;
  wii = new Wrj4P5(this);
  wii.connect(1);
} 

void draw() {
  if (wii.isConnecting()) return;
  background(255);
  fill(0);
  if (!ledsSet) {
    wii.rimokon.setLED(0, true);
    ledsSet = true;
  }
  stroke(0);
  line(0, CONTROLLERS_Y, width, CONTROLLERS_Y);
  drawController("Pitch", wii.rimokon.sensed.y, 75, GREEN);
  drawController("Roll", wii.rimokon.sensed.x, 225, RED);
  drawCursor();
}
  
void drawController(String label, float acc, int xpos, color col) {
  fill(0);
  float deg = asin(acc);
  text(label + ": " +  nf(deg, 1, 3), xpos - 40, CONTROLLERS_Y + 15);
  int len = 40;
  float x = cos(deg) * len;
  float y = sin(deg) * len;
  float ypos = CONTROLLERS_Y + 70;
  fill(col);
  line(xpos - x, ypos - y, xpos + x, ypos + y);
  ellipse(xpos - x, ypos - y, 10, 10);
  ellipse(xpos + x, ypos + y, 10, 10);
}
  
void drawCursor() {
  noStroke();
  fill(0);
  px += wii.rimokon.stableRoll() * -3;
  if (px > width) {
    px = width;
  }
  if (px < 0) {
    px = 0;
  }
  py += wii.rimokon.stablePitch() * -3;
  if (py > CONTROLLERS_Y) {
    py = CONTROLLERS_Y;
  }
  if (py < 0) {
    py = 0;
  }
  ellipse(px, py, 20, 20);  
}
